package Controller;
import lejos.nxt.*;
import java.io.*;

public class OrangeController {
	//DEFAULTS, CONSTANTS & SETTINGS
	boolean VERBOSE = true;
	double DEF_LCIRC = 17.19;
	double DEF_RCIRC = 17.105;
	double DEF_AXIS = 16.1509;
	int TXT_POSX = 1;
	int TXT_POSY = LCD.DISPLAY_CHAR_DEPTH-2;
	int DEF_TURN_SPEED = 350;
	int DEF_DRIVE_SPEED = 400;

	//fields
	private double lcirc; //circumference,
	private double rcirc; //circumference,
	private double axis; //distance between the centers of the 2 wheels
	private double turn; // turning circle around the center
	private int spd_turn;
	private int spd_drive;

	/**
	*Constructor
	* wheel_circ : wheel circumference in centimeters
	* axis_width : axis width in centimeters
	* turn_speed : rotational speed during turning
	* drive_speed : rotational speed during driving
	*/
	public OrangeController(double wheel_lcirc, double wheel_rcirc, double axis_width, int turn_speed, int drive_speed) {
		lcirc = wheel_lcirc;
		rcirc = wheel_rcirc;
		axis = axis_width;
		spd_turn = turn_speed;
		spd_drive = drive_speed;
		turn = axis*Math.PI;
	}
	
	/**
	*Constructor
	* using default values for circumference and axis
	*/
	public OrangeController() {
		//LCD.drawString("Use Calibration data?",0,0);
		//int b = Button.waitForPress();
		//boolean load = false;
		/*try {
			/*if(b==1) {
				File f = new File("calib.dat");
				if(!f.exists()) throw new FileNotFoundException();
				FileInputStream fis = new FileInputStream(f);
				long bits = 0;
				byte[] bytes = new byte[8];
				fis.read(bytes, 0, 8);
				for(int i=0;i<8;i++) {
					bits = (bits<<8)+(long)(bytes[i]);
				}
				double circ = Double.longBitsToDouble(bits);
				bits = 0;
				fis.read(bytes, 0, 8);
				for(int i=0;i<8;i++) {
					bits = (bits<<8)|bytes[i];
				}
				double axis = Double.longBitsToDouble(bits);
				long bits = 0L;
				byte byt = 0;
				for(int i=0;i<8;i++) {
					byt = (byte)(fis.read());
					bits = (bits<<8)+(long)(byt);
				}
				circ = Double.longBitsToDouble(bits);
				bits = 0L;
				for(int i=0;i<8;i++) {
					byt = (byte)(fis.read());
					bits = (bits<<8)+(long)(byt);
				}
				axis = Double.longBitsToDouble(bits);
				fis.close();
				LCD.clearDisplay();
				LCD.drawString("Loaded values:",0,0);
				LCD.drawString("Circ:"+circ,0,1);
				LCD.drawString("Axis:"+axis,0,2);
				Button.waitForPress();
				load = true;
			}
		} catch(Exception e) {
			LCD.drawString("Load error.     ",0,0, true );
			Button.waitForPress();
		}
		if(!load) {
			circ = DEF_CIRC;
			axis = DEF_AXIS;
		}*/
		lcirc = DEF_LCIRC;
		rcirc = DEF_RCIRC;
		axis = DEF_AXIS;
		spd_turn = DEF_TURN_SPEED;
		spd_drive = DEF_DRIVE_SPEED;
		turn = axis*Math.PI;
	}
	
	/**
	*Drive forward
	* dist : distance in cm
	*/
	public void forward(double dist) {
		double factor = lcirc/rcirc;
		Motor.C.setSpeed((int)(spd_drive*factor));
		Motor.B.setSpeed((int)(spd_drive));	
		int rotB = (int)((dist/lcirc)*360.0);
		int rotC = (int)((dist/rcirc)*360.0);
		if(VERBOSE) LCD.drawString("Forward",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rotB,true);
		Motor.C.rotate(rotC);
		if(VERBOSE) LCD.drawString("       ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Drive backward
	* dist : distance in cm
	*/
	public void backward(double dist) {
		double factor = lcirc/rcirc;
		Motor.C.setSpeed((int)(spd_drive*factor));
		Motor.B.setSpeed((int)(spd_drive));	
		int rotB = (int)((dist/lcirc)*-360.0);
		int rotC = (int)((dist/rcirc)*-360.0);
		if(VERBOSE) LCD.drawString("Backward",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rotB,true);
		Motor.C.rotate(rotC);
		if(VERBOSE) LCD.drawString("         ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Turn CCW
	* angle : angle in degrees
	*/
	public void turnCCW(double ang) {
		double factor = lcirc/rcirc;
		Motor.C.setSpeed((int)(spd_drive*factor)+1);
		Motor.B.setSpeed((int)(spd_drive));	
		int rotB = (int)((ang*turn)/lcirc);
		int rotC = (int)((ang*turn)/rcirc);
		if(VERBOSE) LCD.drawString("Turn CCW",TXT_POSX,TXT_POSY);
		Motor.B.rotate(-rotB,true);
		Motor.C.rotate(rotC);
		if(VERBOSE) LCD.drawString("        ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Turn CW
	* angle : angle in degrees
	*/
	public void turnCW(double ang) {
		double factor = lcirc/rcirc;
		Motor.C.setSpeed((int)(spd_drive*factor));
		Motor.B.setSpeed((int)(spd_drive));	
		int rotB = (int)((ang*turn)/lcirc);
		int rotC = (int)((ang*turn)/rcirc);
		if(VERBOSE) LCD.drawString("Turn CW",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rotB,true);
		Motor.C.rotate(-rotC);
		if(VERBOSE) LCD.drawString("       ",TXT_POSX,TXT_POSY);
	}
	
	public double getAxis() { return axis; }
	public double getLCirc() { return lcirc; }
	public double getRCirc() { return rcirc; }
}